#include "utilities.h"
#include "optimiz.h"
#include "lk.h"
#include "free.h"
#include "models.h"
#include "mc.h"
#include "rates.h"
#include "annealing.h"
#include "numeric.h"
#include "spr.h"
#include "emig.h"

annealing anneal;

// "Set_Anneal" sets the default values of thermal annealing parameters. . .
void Set_Annealing_Defaults(option *io){
	anneal.t_scalar = 0.3; //this one gets set from io later... don't change this line.
	anneal.random_seed = (int)time(NULL);
	anneal.rng = gsl_rng_alloc(gsl_rng_mt19937);
	anneal.no_change = 1;
	anneal.max_alpha = 0.3;
	gsl_rng_set(anneal.rng, anneal.random_seed);
}

void Free_Anneal(){
	gsl_rng_free( anneal.rng );
}


void Swap_Tree_And_Mod(arbre *tree1, arbre *tree2){
	arbre *tmptree;
	model *tmpmod;
	tmptree = tree1;
	tmpmod = tree1->mod;

	tree1 = tree2;
	tree1->mod = tree2->mod;
	tree2 = tmptree;
	tree2->mod = tree1->mod;
}

// This method returns the scaled and estimated acceptance ratio for thermal annealing.
// The variable "tree" will be modified by this method, such that tree will be in a good
// starting position for thermal annealing after this method concludes.
double Scale_T_Factor(arbre *tree){
	if (!tree->io->quiet)
	{
		PhyML_Printf("\n. Scaling temperature.  This could take a while...\n");
	}

	/* psuedo-code:

	lnL_best = tree's lnL
	lnL_proposed = max_lnL

	fsum = 0.0 // function mean
	fsqsum  0.0 // function standard deviation
	n = 0 // number of points tried

	tree_proposed = copy of tree

	for iter in range(0, iters_per_temp):
		Get_Neighbor_Proposition(tree_proposed)
		lnL_proposed = tree_proposed's likelihood
		if (lnL_proposed > -DBL_MAX):
			n++
			fsum += working_lnL
			fsqsum += working_lnL * working_lnL
		if (lnL_proposed > lnL_best):
			lnL_best = lnL_proposed
			tree = copy of tree_proposed

	fsum = fsum / n
	fsqsum = fsqsum / n
	fsqsum = sqrt( fsqsum - fsum * fsum)
	aratio = t_scalar * fsqsum / start_temp
	return aratio
	 */

	double aratio;
	Lk(tree);
	double lnL_best = tree->c_lnL;
	double lnL_proposed = tree->c_lnL;
	double fsum = 0.0;
	double fsqsum = 0.0;
	int n = 0;
	int i;

	//Optimiz_All_Free_Param(tree,0);
	arbre *best_tree = Make_Tree(tree->n_otu);
	Init_Tree(best_tree,tree->n_otu);
	Make_All_Tree_Nodes(best_tree, tree->mod->n_l);
	Make_All_Tree_Edges(best_tree, tree->mod->n_l);
	Make_List_Of_Reachable_Tips(best_tree);
	best_tree->mod = Copy_Model(tree->mod);
	Copy_Tree(tree,best_tree);

	For(i, tree->io->sa_iters_per_stage){
		Get_Neighbor_Proposition(tree,tree->io->sa_temp_start);
		lnL_proposed = tree->c_lnL;
		if(lnL_proposed > UNLIKELY){ // is the proposition valid?
			n++;
			fsum += lnL_proposed;
			fsqsum += (lnL_proposed * lnL_proposed);
		}
		if(lnL_proposed > lnL_best){ // is the proposition better?
			lnL_best = lnL_proposed;
			Copy_Tree(tree,best_tree);
			Record_Model(tree->mod,best_tree->mod);
		}else{
			Copy_Tree(best_tree,tree);
			Record_Model(best_tree->mod,tree->mod);
		}

		if (!tree->io->quiet)
		{
			double a = fsum / (double) n;
			double b = fsqsum / (double) n;
			double c = sqrt(b - a * a);
			PhyML_Printf(" . [ iteration %d or %d] [scalar = %f]\n", i, tree->io->sa_iters_per_stage, anneal.t_scalar * c / tree->io->sa_temp_start);
		}
	}
	fsum /= (double)n; //average proposed likelihood
	fsqsum /= (double)n; //average (squared) proposed likelihood
	fsqsum = sqrt(fsqsum - fsum * fsum);
	aratio = anneal.t_scalar * fsqsum / tree->io->sa_temp_start;

	Free_Model(best_tree->mod);
	Free_Tree(best_tree);

	//now lets get a really good starting position...
	//Speed_Spr_Loop(tree);

	return aratio;
}

// This method modifies some or all parameters of "tree", including:
// * topology
// * branch lengths
// * branch length proportions
// * alpha (for gamma distributed ASRV)
// * gamma proportions
// * mu (evolutionary rate)
void Get_Neighbor_Proposition(arbre *tree,double temp){

	tree->both_sides = 0;
	anneal.no_change = 1;
	double x = gsl_rng_uniform(anneal.rng);
	if(x < tree->io->sa_prob_topo)Step_Topology(tree);
	x = gsl_rng_uniform(anneal.rng);
	if(x < tree->io->sa_prob_kappa)Step_Kappa(tree);
	x = gsl_rng_uniform(anneal.rng);
	if(x < tree->io->sa_prob_lambda)Step_Lambda(tree);
	x = gsl_rng_uniform(anneal.rng);
	if(x < tree->io->sa_prob_rr)Step_RR(tree);
	x = gsl_rng_uniform(anneal.rng);
	if(x < tree->io->sa_prob_pi) Step_Pi(tree);
	x = gsl_rng_uniform(anneal.rng);
	if(x < tree->io->sa_prob_gamma)Step_Gamma(tree);
	x = gsl_rng_uniform(anneal.rng);
	if(x < tree->io->sa_prob_pinvar)Step_Pinvar(tree);
	x = gsl_rng_uniform(anneal.rng);
	if(x < tree->io->sa_prob_brlen)Step_Branch_Lengths(tree,temp);
	x = gsl_rng_uniform(anneal.rng);
	if(x < tree->io->sa_prob_blprop)Step_Brlen_Proportion(tree);

	// Update the likelihood of tree, but only if the model was changed.
	if(anneal.no_change == 0)
	{
		Lk(tree);
	}
	tree->mod->update_eigen = 0;
	tree->both_sides = 1; // reset to 1

	//
	// Print the full proposition to a text file
	//
	if (tree->io->print_trace == 1)
	{
		PhyML_Fprintf(tree->io->fp_out_trace,"[%f]%s\n",tree->c_lnL,Write_Tree(tree,1)); fflush(tree->io->fp_out_trace);

		int i;
		PhyML_Fprintf(tree->io->fp_out_annealing_file,
				". proposition: [lnL = %f]\n",
				tree->c_lnL);
		// BL proportions
		if(tree->mod->s_opt->opt_blprops){
			PhyML_Fprintf(tree->io->fp_out_annealing_file, "\t[BL proportions] [");
			For(i, tree->mod->n_l)
			{
				PhyML_Fprintf(tree->io->fp_out_annealing_file, "%.3f ", tree->mod->bl_props[i]);
			}
			PhyML_Fprintf(tree->io->fp_out_annealing_file, "]\n");
		}

		// tree->mod->alpha
		if(tree->mod->s_opt->opt_alpha == 1){
			PhyML_Fprintf(tree->io->fp_out_annealing_file, "\t[alpha] [%.3f]\n", tree->mod->alpha);
		}

		// tree->mod->pinvar
		if(tree->mod->s_opt->opt_pinvar == 1){
			PhyML_Fprintf(tree->io->fp_out_annealing_file, "\t[prob. invar.] [%.3f]\n", tree->mod->pinvar);
		}

		// tree->mod->kappa
		if(tree->mod->s_opt->opt_kappa == 1){
			PhyML_Fprintf(tree->io->fp_out_annealing_file, "\t[kappa] [%.3f]\n", tree->mod->kappa);
		}

		// tree->mod->lambda
		if(tree->mod->s_opt->opt_lambda == 1){
			PhyML_Fprintf(tree->io->fp_out_annealing_file, "\t[lambda] [%.3f]\n", tree->mod->lambda);
		}

		// relative rate params. for GTR
		if(((tree->mod->whichmodel == GTR) && (tree->mod->s_opt->opt_rr == 1)) ||
				((tree->mod->whichmodel == CUSTOM) && (tree->mod->s_opt->opt_rr == 1)) )
		{
			PhyML_Fprintf(tree->io->fp_out_annealing_file, "\t[relative rates] [");
			for(i = 0; i < tree->mod->n_diff_rr; i++){
				PhyML_Fprintf(tree->io->fp_out_annealing_file, "%.3f ", tree->mod->rr_val[i]);
			}
			PhyML_Fprintf(tree->io->fp_out_annealing_file, "]\n");
		}

		// state frequencies
		if((tree->mod->s_opt->opt_state_freq) /*&& (tree->mod->datatype == NT)*/)
		{
			PhyML_Fprintf(tree->io->fp_out_annealing_file, "\t[state freqs.] [");
			for(i = 0; i < tree->mod->ns; i++){
				PhyML_Fprintf(tree->io->fp_out_annealing_file, "%.3f ", tree->mod->pi_unscaled[i]);
			}
			PhyML_Fprintf(tree->io->fp_out_annealing_file, "]\n");
		}

		fflush(tree->io->fp_out_annealing_file);
	}
}

void Step_Brlen_Proportion(arbre *tree){
	if(tree->mod->s_opt->opt_blprops)
	{
		if (tree->io->sa_sele_blprop == 0)
		{
			int failed;
			failed = 0;
			BFGS(tree,tree->mod->bl_props,tree->mod->n_l,1.e-5,1.e-7,
					&Return_Abs_Lk,
					&Num_Derivative_Several_Param,
					&Lnsrch_BLP_Param,&failed);
			if(failed)
			{
				int i;
				For(i,tree->mod->n_l)
				{
					Optimize_Single_Param_Generic(tree,&(tree->mod->bl_props[i]),
							1.E-2,1.E+2,
							tree->mod->s_opt->min_diff_lk_global,
							tree->mod->s_opt->brent_it_max,
							tree->mod->s_opt->quickdirty);
				}
			}
		}
		else if (tree->io->sa_sele_blprop == 1)
		{
			int max = tree->io->sa_sele_max_params;
			if (max < 1) max = 1;
			int n_to_modify = gsl_rng_uniform_int(anneal.rng,max);
			if(n_to_modify < 1) n_to_modify = 1; // we'll perturb the values of this many BL props

			int i;
			For(i,n_to_modify)
			{
				// select a random BL proportion value (r_i)
				int r_i = gsl_rng_uniform_int(anneal.rng, tree->mod->n_l);

				double r;
				// select a random perturbation value, r
				r = gsl_ran_gaussian(anneal.rng,tree->io->sa_sigma_blprop);
				tree->mod->bl_props[r_i] += r;
				if(tree->mod->bl_props[r_i] < 0.001) tree->mod->bl_props[r_i] = 0.001;
				else if(tree->mod->bl_props[r_i] > 100.0) tree->mod->bl_props[r_i] = 99.9999;
			}
		}
		else if (tree->io->sa_sele_blprop == 2)
		{
            double *alpha;
            alpha = (double *)mCalloc(tree->mod->n_l, sizeof(double));
            int i;
            For(i,tree->mod->n_l){
            	alpha[i] = anneal.max_alpha;
            }
            gsl_ran_dirichlet(anneal.rng,tree->mod->n_l,alpha,tree->mod->bl_props);
            Free(alpha);
		}

		// and finally...
        tree->both_sides = 1;
        anneal.no_change = 0;
        Normalize_Props(tree->mod);
	}
}


void Step_Pinvar(arbre *tree){
	if(tree->mod->s_opt->opt_pinvar == 1){
		if (tree->io->sa_sele_pinvar == 0)
		{
			Optimize_Single_Param_Generic(tree,&(tree->mod->pinvar),
					.0001,0.9999,
					tree->mod->s_opt->min_diff_lk_global,
					tree->mod->s_opt->brent_it_max,
					tree->mod->s_opt->quickdirty);
		}
		if (tree->io->sa_sele_pinvar == 1)
		{
			double r;
			anneal.no_change = 0;
			r = gsl_ran_gaussian(anneal.rng,tree->io->sa_sigma_pinvar);
			tree->mod->pinvar += r;
			if(tree->mod->pinvar < 0.0001) tree->mod->pinvar = 0.0001;
			else if(tree->mod->pinvar > 0.9999) tree->mod->pinvar = 0.9999;
		}
	}
}

void Step_RR(arbre * tree){
	if(((tree->mod->whichmodel == GTR) && (tree->mod->s_opt->opt_rr == 1) ) ||
			((tree->mod->whichmodel == CUSTOM) && (tree->mod->s_opt->opt_rr == 1)))
	{
		if (tree->io->sa_sele_rr == 0) // Use hill climbing instead of random perturbation
		{
			int failed;
			failed = 0;
			tree->mod->update_eigen = 1;
			anneal.no_change = 0;
			BFGS(tree,tree->mod->rr_val,tree->mod->n_diff_rr,1.e-5,1.e-7,
					&Return_Abs_Lk,
					&Num_Derivative_Several_Param,
					&Lnsrch_RR_Param,&failed);
			if(failed)
			{
				int i;

				For(i,tree->mod->n_diff_rr)
				if(i != 5)
				{
					Optimize_Single_Param_Generic(tree,&(tree->mod->rr_val[i]),
							1.E-2,1.E+2,
							tree->mod->s_opt->min_diff_lk_global,
							tree->mod->s_opt->brent_it_max,
							tree->mod->s_opt->quickdirty);
				}

			}
		}
		else if (tree->io->sa_sele_rr == 1) // use a gaussian distribution
		{
			int r_i = gsl_rng_uniform_int(anneal.rng, tree->mod->n_diff_rr);// select a random RR value to perturb

			//PhyML_Printf(". debug annealing.c: 350, r_i = %d\n", r_i);
			double r;
			anneal.no_change = 0;
			tree->mod->update_eigen = 1;
			r = gsl_ran_gaussian(anneal.rng,tree->io->sa_sigma_rr);
			tree->mod->rr_val[r_i] += r;
			if(tree->mod->rr_val[r_i] < 0.001) tree->mod->rr_val[r_i] = 0.001;
			else if(tree->mod->rr_val[r_i] > 100.0) tree->mod->rr_val[r_i] = 99.9999;
		}

	}
}


void Step_Kappa(arbre * tree){
	if(tree->mod->s_opt->opt_kappa == 1){
		if (tree->io->sa_sele_kappa == 0)
		{
			tree->mod->update_eigen = 1;
			anneal.no_change = 0;
			Optimize_Single_Param_Generic(tree,&(tree->mod->kappa),
					.1,100.,
					tree->mod->s_opt->min_diff_lk_global,
					tree->mod->s_opt->brent_it_max,
					tree->mod->s_opt->quickdirty);
		}
		else if (tree->io->sa_sele_kappa == 1) // use a gaussian distribution
		{
			double r;
			tree->mod->update_eigen = 1;
			anneal.no_change = 0;
			r = gsl_ran_gaussian(anneal.rng,tree->io->sa_sigma_kappa);
			tree->mod->kappa += r;
			if(tree->mod->kappa < 0.1) tree->mod->kappa = 0.1;
			else if(tree->mod->kappa > 100.0) tree->mod->kappa = 99.9999;
		}
	}
}


void Step_Lambda(arbre * tree){
	if(tree->mod->s_opt->opt_lambda == 1){
		if (tree->io->sa_sele_lambda == 0)
		{
			anneal.no_change = 0;
			tree->mod->update_eigen = 1;
			Optimize_Single_Param_Generic(tree,&(tree->mod->lambda),.001,100.,
					tree->mod->s_opt->min_diff_lk_global,
					tree->mod->s_opt->brent_it_max,
					tree->mod->s_opt->quickdirty);
		}
		else if (tree->io->sa_sele_lambda == 1) // use a gaussian distribution
		{
			double r;
			anneal.no_change = 0;
			tree->mod->update_eigen = 1;
			r = gsl_ran_gaussian(anneal.rng,tree->io->sa_sigma_lambda);
			tree->mod->lambda += r;
			if(tree->mod->lambda < 0.001) tree->mod->lambda = 0.001;
			else if(tree->mod->lambda > 100.0) tree->mod->lambda = 99.9999;
		}
	}
}


void Step_Gamma(arbre *tree){
	if((tree->mod->s_opt->opt_alpha == 1) && (tree->mod->n_catg > 1)){
		if (tree->io->sa_sele_gamma == 0)
		{
			anneal.no_change = 0;
			Optimize_Single_Param_Generic(tree,&(tree->mod->alpha),
					tree->mod->alpha/2.,tree->mod->alpha*2.,
					tree->mod->s_opt->min_diff_lk_global,
					tree->mod->s_opt->brent_it_max,
					tree->mod->s_opt->quickdirty);
		}
		else if (tree->io->sa_sele_gamma == 1) // use a gaussian distribution
		{
			anneal.no_change = 0;
			double r = gsl_ran_gaussian(anneal.rng,tree->io->sa_sigma_gamma);
			tree->mod->alpha += r;
			if(tree->mod->alpha < 0.01) tree->mod->alpha = 0.01;
			else if(tree->mod->alpha > 100.0) tree->mod->alpha = 100.0;
		}
	}
}

void Step_Pi(arbre * tree){
	if((tree->mod->s_opt->opt_state_freq) /*&& (tree->mod->datatype == NT)*/){
		//PhyML_Printf(". debug: annealing.c:443, optimize pi!\n");

		if (tree->io->sa_sele_pi == 1) // use a gaussian distribution
		{
			int max = tree->io->sa_sele_max_params;
			if (max < 1) max = 1;
			int n_pi_to_modify = gsl_rng_uniform_int(anneal.rng,max);
			if(n_pi_to_modify < 1) n_pi_to_modify = 1; // we'll perturb the values of this many pi vals

			int i;
			For(i,n_pi_to_modify){
				// select a random pi value (r_i)
				double x = gsl_rng_uniform(anneal.rng);
				int r_i = (int)x%tree->mod->n_l;

				double r;
				anneal.no_change = 0;
				tree->mod->update_eigen = 1;
				// select a random perturbation value, r
				r = gsl_ran_gaussian(anneal.rng,tree->io->sa_sigma_pi);
				//PhyML_Printf(". debug: annealing.c:452, proposing new gaussian, r = %f.\n", r);
				tree->mod->pi_unscaled[r_i] += r;
				if(tree->mod->pi_unscaled[r_i] < 0.001) tree->mod->pi_unscaled[r_i] = 0.001;
				else if(tree->mod->pi_unscaled[r_i] > 100.0) tree->mod->pi_unscaled[r_i] = 99.9999;
			}

		}
		if (tree->io->sa_sele_pi == 0)
		{
			int failed,i;

			failed = 0;
			tree->mod->update_eigen = 1;
			anneal.no_change = 0;
			BFGS(tree,tree->mod->pi,
					tree->mod->ns,
					1.e-5,
					1.e-7,
					&Return_Abs_Lk,
					&Num_Derivative_Several_Param,
					&Lnsrch_Nucleotide_Frequencies,
					&failed);

			if(failed)
			{
				For(i,tree->mod->ns)
				Optimize_Single_Param_Generic(tree,&(tree->mod->pi_unscaled[i]),
						-1000.,1000.,
						tree->mod->s_opt->min_diff_lk_global,
						tree->mod->s_opt->brent_it_max,
						tree->mod->s_opt->quickdirty);
			}
			tree->mod->update_eigen = 0;
		}
		else if (tree->io->sa_sele_blprop == 2)
		{
			tree->mod->update_eigen = 1;
			anneal.no_change = 0;
            double *alpha;
            alpha = (double *)mCalloc(tree->mod->ns, sizeof(double));
            int i;
            For(i,tree->mod->ns){
            	alpha[i] = anneal.max_alpha;
            }
            gsl_ran_dirichlet(anneal.rng,tree->mod->ns,alpha,tree->mod->pi_unscaled);
            Free(alpha);
		}
	}
}

// helper for "Get_Neighbor_Proposition"
void Step_Branch_Lengths(arbre *tree, double temp){
	if(tree->mod->s_opt->opt_bl == 1)
	{
		if (tree->io->sa_sele_bl == 0) // hill-climbing
		{
			Optimize_Br_Len_Serie(tree->noeud[0],
									   tree->noeud[0]->v[0],
									   tree->noeud[0]->b[0],
									   tree,
									   tree->data);
			tree->both_sides = 1;
			anneal.no_change = 0;
		}
		else if(tree->io->sa_sele_bl == 1) // Gaussian random perturbation
		{
			tree->both_sides = 1;
			anneal.no_change = 0;
			int i,j,n_edges_to_modify,n,max;
			int n_edges = (tree->n_otu * 2) - 3;
			double rand_gauss;

			//max = (n_edges + 1) * (tree->mod->n_l + 1) * gsl_rng_uniform(anneal.rng);
			max = tree->io->sa_sele_max_params;
			if (max < 1) max = 1;
			n_edges_to_modify = gsl_rng_uniform_int(anneal.rng,max);
			if(n_edges_to_modify < 1) n_edges_to_modify = 1; // we'll perturb the lengths of this many edges

			For(i,n_edges_to_modify){
				j = gsl_rng_uniform_int(anneal.rng,n_edges); // j is a random edge
				rand_gauss = gsl_ran_gaussian(anneal.rng,tree->io->sa_sigma_bl); // get a random perturbation delta
				n = gsl_rng_uniform_int(anneal.rng,tree->mod->n_l); // n is a random BL set
				tree->t_edges[j]->l[n] += rand_gauss; // perturb branch j's nth value
				//PhyML_Printf(" . debug annealing.c edge %d, mixture %d, length += %f\n", j, n, rand_gauss);
				if(tree->t_edges[j]->l[n] < BL_MIN) tree->t_edges[j]->l[n] = BL_MIN;
				else if(tree->t_edges[j]->l[n] > BL_MAX) tree->t_edges[j]->l[n] = BL_MAX;

			}
		}
	}
}


// helper for "Get_Neighbor_Proposition"
void Step_Topology(arbre *tree){
	// invoke NNI or SPR to create a new topology.
	// JSJ: pick a random edge, from that edge find a path connecting 4 nodes
	//       swap those four nodes.
	if(tree->mod->s_opt->opt_topo)
	{
		anneal.no_change = 0;

		double p = gsl_rng_uniform(anneal.rng);
		if(p <= tree->io->sa_prob_NNI)
		{
			Simu(tree,1);
		}
		else
		{
			Random_Spr(10,tree);
		}
	}
}


void Get_QA_Neighbor_Proposition(arbre *tree){

}

// Boltzmann probability sampler, for thermal annealing
//
// INPUT: Given the likelihood (lnl_curr) of our current state, the likelihood (lnl_new) of our new proposed state,
// and the temperature (temperature) of the system, calculate the probability of the new state,
// where the probability is drawn from a Boltzmann distribution.
//
// OUTPUT: a floating point decimal, between 0.0 and 1.0
double Boltzmann_P_TA(double lnl_curr, double lnl_new, double temperature){
	double result = 1.0;
	if(lnl_new < lnl_curr){
		result = exp((lnl_new - lnl_curr)/(anneal.t_scalar * temperature));
	}
	//PhyML_Printf("In Boltzmann_P: t_scalar= %lf, lnl_curr = %lf, lnl_new = %lf, temp= %lf, result = %lf\n",anneal.t_scalar,lnl_curr,lnl_new,temperature,result);
	return result;
}

//Boltzmann probability sampler, for quantum annealing
//
// distance = the distance between the proposed state and the current state
// tfield = the transverse field value (tau), which is QA's analog to the temperature in TA.
//
double Boltzmann_P_QA(double lnl_curr, double lnl_new, double distance, double tfield){
	double result = 1.0;
	if(lnl_new < lnl_curr){
		result = exp(distance / (anneal.t_scalar * tfield));
	}
	return result;
}



// INPUT: a tree structure, where tree->mod->s_opt contains the
//      parameters which are free to be optimized.
// OUTPUT: the likelihood of the best found tree
//
// At the end of this method, the object "tree" will contain the best-found topology,
//      branch lengths, and model parameters.  In other words, tree is mutable!
double Thermal_Anneal_All_Free_Params(arbre *tree, int verbose){

	Set_Annealing_Defaults(tree->io);
	//double result = 1.0;
	tree->mod->update_eigen = 1;
	tree->both_sides = 1; //search both pre and post order on all subtrees
	int n_edges = (tree->n_otu * 2) - 3;
	if (n_edges <= 3){
		tree->mod->s_opt->opt_topo = 0; //make sure that opt_topo is false if there are no meaningful branch swaps.
	}

	//
	// Pre-optimize STA?
	//
	if (tree->io->sa_preopt > -1)
	{
		if (!tree->io->quiet)
		{	PhyML_Printf("\n. Before STA begins: pre-optimizing topology, branch lengths, and model parameters.\n");
		}

		tree->io->opt_algorithm = tree->io->sa_preopt;
		Simu_Loop(tree);
		Print_Tree_Screen( tree );

		//
		// Print the pre-opt tree:
		//
		char pfilestr[1000];
		sprintf(pfilestr, "%s.preopt", tree->io->out_tree_file);
		FILE* pfile = fopen(pfilestr, "w");
		if(!pfile)
		{
			fprintf(stderr, "ERROR: cannot open output file: %s\n",
						pfilestr);
			exit(1);
		}
		char *most_likely_trees = Write_Tree(tree,1);
		PhyML_Fprintf(pfile,"%s\n",most_likely_trees);
		fclose(pfile);

		//
		// Print the pre-opt stats file:
		//
		char pfilestr2[1000];
		sprintf(pfilestr2, "%s.preopt", tree->io->out_stats_file);
		pfile = fopen(pfilestr2, "w");
		if(!pfile)
		{
			fprintf(stderr, "ERROR: cannot open output file: %s\n",
						pfilestr2);
			exit(1);
		}
		time_t t_end;
		time(&t_end);
		Print_Fp_Out(pfile,tree->t_beg,t_end,tree,tree->io,1,1);
		fclose(pfile);

		//
		// Print the pre-opt per-site lnls
		//
		char pfilestr3[1000];
		sprintf(pfilestr3, "%s.preopt", tree->io->out_lk_file);
		pfile = fopen(pfilestr3, "w");
		if(!pfile)
		{
			fprintf(stderr, "ERROR: cannot open output file: %s\n",
						pfilestr3);
			exit(1);
		}
		if(tree->io->print_site_lnl)
		{
			Print_Site_Lk(tree,pfile);
		}
		fclose(pfile);

		//
		// Print the per-opt sitecat lks
		//
		char sitecatlkfilestr[1000];
		sprintf(sitecatlkfilestr, "%s_phyml_sitecatlks.txt.preopt", tree->io->in_seq_file);
		FILE* sitecatfp = fopen(sitecatlkfilestr, "w");
		Print_Sitecat_Lks(tree, sitecatfp);
		close(sitecatfp);

		// Finally, return optimization algorithm to STA.
		tree->io->opt_algorithm = STA_ALG;
	}

	double temp = tree->io->sa_temp_start;
	double tempmult = exp(log(tree->io->sa_temp_end/tree->io->sa_temp_start)/(((double)tree->io->sa_num_anneal_stages) - 1.0));
	if(tree->io->sa_scale_temp == 1)
	{
		anneal.t_scalar = Scale_T_Factor(tree);
		if (anneal.t_scalar < 0.1) // VHS: a hack?
		{
			anneal.t_scalar = 0.1;
		}
	}
	else{
		anneal.t_scalar = 1.0; //if positive, user input a value, don't estimate.
		Lk(tree);
#ifdef MEASURE
		//Count_Mean_Compressability(tree);
#endif

	}

	//
	arbre *best_tree = Make_Tree(tree->n_otu);
	Init_Tree(best_tree,tree->n_otu);
	Make_All_Tree_Nodes(best_tree, tree->mod->n_l);
	Make_All_Tree_Edges(best_tree, tree->mod->n_l);
	Make_List_Of_Reachable_Tips(best_tree);
	best_tree->mod = Copy_Model(tree->mod);
	Copy_Tree(tree,best_tree);

	//
	arbre *last_tree = Make_Tree(tree->n_otu);
	Init_Tree(last_tree,tree->n_otu);
	Make_All_Tree_Nodes(last_tree, tree->mod->n_l);
	Make_All_Tree_Edges(last_tree, tree->mod->n_l);
	Make_List_Of_Reachable_Tips(last_tree);
	last_tree->mod = Copy_Model(best_tree->mod);


	/*
	best_tree->c_lnL = tree->c_lnL;
	Prepare_Tree_For_Lk(best_tree);
	Lk(best_tree);
	*/

	time_t start = time(NULL);
	time_t now;

	double lnL_best = tree->c_lnL;
	double lnL_current = tree->c_lnL;
	double lnL_proposed = tree->c_lnL;
	double acc_prob;
	double r;
	int itemp,iter;
	int total_steps_tried = 0;
	int total_steps_accepted = 0;

	double temp_of_best = 0; // VHS: for optimization purposes, let's record the temperature at which we discovered the best tree.
	double iter_of_best = 0;

	//PhyML_Printf("\n\n Starting simulated thermal annealing with the following dynamic parameter:\n\n");
	if(tree->io->sa_scale_temp == 1)
	{
		//PhyML_Printf("\t. temperature scalar = %f\n", anneal.t_scalar);
	}
	//PhyML_Printf("\t. temperature multiplier = %f\n\n", tempmult);


	for(itemp = 0; itemp < tree->io->sa_num_anneal_stages; itemp++){

		int steps_tried = 0.0;
		int steps_accepted = 0.0;

		//recenter the search at each temperature.
		//make the tree our best tree so far
		Copy_Tree(best_tree,tree);
		Record_Model(best_tree->mod,tree->mod);
		Copy_Tree(tree,last_tree);
		Record_Model(tree->mod,last_tree->mod);
		lnL_current = lnL_best;

		for(iter = 0; iter < tree->io->sa_iters_per_stage; iter++){
			steps_tried++;
			//get our next proposition.
			Get_Neighbor_Proposition(tree,temp);
			lnL_proposed = tree->c_lnL;
#ifdef MEASURE
			Count_Mean_Compressability(tree);
#endif

			now = time(NULL);
			double save_lnL_current = (double)lnL_current;

			if (tree->io->measure)
			{
				PhyML_Printf("plot1 %d %lf\n", steps_tried, lnL_current);
				PhyML_Printf("plot2 %ld %d\n", (long)difftime(now,start), total_steps_accepted);
			}

			//PhyML_Printf("proposed tree = %s\n", Write_Tree(tree) );

			if(lnL_proposed > lnL_best){
				//save this tree into best_tree
				//PhyML_Printf("(annealing.c, 465): Found a new best tree with lnL = %f\n", lnL_best);
				Copy_Tree(tree,best_tree);
				Record_Model(tree->mod, best_tree->mod);
				lnL_best = lnL_proposed;
				temp_of_best = temp;
				iter_of_best = iter;

				// This temperature is yielding good results: let's stay here
				// longer, thus increasing our chances of reaching the best ground
				// state at this temperature.
				iter -= tree->io->sa_set_back;
				if(iter < 0) iter = 0; //make sure not set back into negative...
				acc_prob = 1.0;
				r = 0.0;
			}
			else if(lnL_proposed > lnL_current){
				acc_prob = 1.0;
				r = 0.0;
			}else{
				acc_prob = Boltzmann_P_TA(lnL_current, lnL_proposed, temp);
				r = gsl_rng_uniform(anneal.rng);
			}

			if(acc_prob >= r){
				//save the current tree
				if(anneal.no_change == 0){
					Copy_Tree(tree,last_tree);
					Record_Model(tree->mod,last_tree->mod);
				}
				steps_accepted++;
				lnL_current = tree->c_lnL;

				//PhyML_Printf("JSJ: Proposed Likelihood Accepted, acc_prob = %lf\n",acc_prob);

			}else{
				//restore the current tree to be our last tree
				if(anneal.no_change == 0){
					Copy_Tree(last_tree,tree);
					Record_Model(last_tree->mod,tree->mod);
				}
			}

			// calculate acceptance ratio
			double acc_ratio = (double)steps_accepted / (double)steps_tried;

			PhyML_Printf("[%ld sec] [temp. = %f] [current lnL = %.4f] [proposed lnL = %.4f] [p = %.3f] [accepted/proposed = %.3f]\n", (long)difftime(now,start), temp, save_lnL_current, lnL_proposed, acc_prob, acc_ratio);

		}//end inner for loop.
		temp *= tempmult;

		total_steps_tried += steps_tried;
		total_steps_accepted += steps_accepted;

	}
	if(verbose){
		PhyML_Printf("Annealing finished.\n");
		PhyML_Printf("In temperature range [%f, %f], the best tree was found at %f, iteration %f\n", temp, tree->io->sa_temp_start, temp_of_best, iter_of_best);
	}
	Copy_Tree(best_tree,tree);
	Record_Model(best_tree->mod,tree->mod);

	Free_Model(best_tree->mod);
	Free_Tree(best_tree);
	Free_Model(last_tree->mod);
	Free_Tree(last_tree);
	Free_Anneal(); //local function that frees the gsl_rng;
	Lk(tree);

	return tree->c_lnL;
}


// INPUT: a tree structure, where tree->mod->s_opt contains the
//      parameters which are free to be optimized.
// OUTPUT: the likelihood of the best found tree
//
// At the end of this method, the object "tree" will contain the best-found topology,
//      branch lengths, and model parameters.  In other words, tree is mutable!
/*
double Quantum_Anneal_All_Free_Params(arbre *tree, int verbose){

	Set_Anneal(tree->io);
	//double result = 1.0;
	tree->mod->update_eigen = 1;
	tree->both_sides = 1; //search both pre and post order on all subtrees
	int n_edges = (tree->n_otu * 2) - 3;
	if (n_edges <= 3){
		tree->mod->s_opt->opt_topo = 0; //make sure that opt_topo is false if there are no meaningful branch swaps.
	}
	double tau = anneal.start_tau;
	double taumult = exp(log(anneal.end_tau/anneal.start_tau)/(((double)io->sa_num_anneal_stages) - 1.0));
	if(tree->io->sa_acc_ratio < 0.0)
	{
		anneal.t_scalar = Scale_T_Factor(tree);
	}
	else{
		anneal.t_scalar = tree->io->sa_acc_ratio; //if positive then the user inputed a value, and we shouldn't estimate.
		Lk(tree);
	}

	Optimiz_All_Free_Param(tree,1);
	//printf("(annealing.c 742) anneal.sa_num_anneal_stages = %d", anneal.sa_num_anneal_stages);

	arbre *best_tree = Make_Tree(tree->n_otu);
	Init_Tree(best_tree,tree->n_otu);
	Make_All_Tree_Nodes(best_tree, tree->mod->n_l);
	Make_All_Tree_Edges(best_tree, tree->mod->n_l);
	best_tree->mod = Copy_Model(tree->mod);
	Copy_Tree(tree,best_tree);

	//Speed_Spr_Loop(tree);
	arbre *last_tree = Make_Tree(tree->n_otu);
	Init_Tree(last_tree,tree->n_otu);
	Make_All_Tree_Nodes(last_tree, tree->mod->n_l);
	Make_All_Tree_Edges(last_tree, tree->mod->n_l);
	last_tree->mod = Copy_Model(best_tree->mod);
	time_t start = time(NULL);
	time_t now;

	double lnL_best = tree->c_lnL;
	double lnL_current = tree->c_lnL;
	double lnL_proposed = tree->c_lnL;
	double acc_prob;
	double r;
	int itau,iter;
	int steps_tried = 0;
	int steps_accepted = 0;

	double tau_of_best = 0; // VHS: for optimization purposes, let's record the tau at which we discovered the best tree.
	double iter_of_best = 0;

	PhyML_Printf("\n\n Starting simulated quantum annealing with the following parameters:\n");
	PhyML_Printf("\t. acceptance ratio = %f\n", anneal.t_scalar);
	PhyML_Printf("\t. starting tau value = %f\n", anneal.start_tau);
	PhyML_Printf("\t. ending tau value = %f\n", anneal.end_tau);
	PhyML_Printf("\t. tau count = %d\n", io->sa_num_anneal_stages);
	PhyML_Printf("\t. tau multiplier = %f\n", taumult);

	PhyML_Printf("\t. P of adjusting alpha = %f\n", anneal.prob_gamma );
	PhyML_Printf("\t. a.s.r.v. gamma sigma = %f\n", anneal.gamma_sigma);
	PhyML_Printf("\t. lambda sigma = %f\n", anneal.lambda_sigma);
	PhyML_Printf("\t. kappa sigma = %f\n", anneal.kappa_sigma);

	PhyML_Printf("\t. P of adjusting branch lengths = %f\n", anneal.prob_brlen );
	PhyML_Printf("\t. branch length sigma = %f\n", anneal.brlen_sigma);

	PhyML_Printf("\t. P of adjusting pinvar = %f\n", anneal.prob_pinvar );
	PhyML_Printf("\t. pinvar. sigma = %f\n", anneal.pinvar_sigma);

	PhyML_Printf("\t. P of changing topology = %f\n", anneal.prob_topology );
	PhyML_Printf("\t. P of using TBR = %f\n", anneal.prob_TBR );
	PhyML_Printf("\t. P of using NNI = %f\n", anneal.prob_NNI );
	PhyML_Printf("\t. P of using SPR = %f\n", anneal.prob_SPR );

	PhyML_Printf("\t. P of adjusting branch length rate proportions = %f\n", anneal.prob_rate_proportion );

	//double  sa_prob_pi;
	//double  sa_prob_kappa;
	//double  sa_prob_lambda;
	//double  sa_prob_rr;
	//double  prob_emig;


	for(itau = 0; itau < io->sa_num_anneal_stages; itau++){
		//recenter the search at each tau.
		//make the tree our best tree so far
		//
		// VHS: This recentering step might be meretricious.
		// Let's examine the run without this step.
		Copy_Tree(best_tree,tree);
		Record_Model(best_tree->mod,tree->mod);
		Copy_Tree(tree,last_tree);
		Record_Model(tree->mod,last_tree->mod);
		lnL_current = lnL_best;

		//steps_tried = 0;
		//steps_accepted = 0;

		//Optimiz_All_Free_Param(tree,1);

		for(iter = 0; iter < anneal.iters_per_temp; iter++){
			steps_tried++;
			//get our next proposition.
			Get_Neighbor_Proposition(tree,tau);
			lnL_proposed = tree->c_lnL;

			now = time(NULL);
			// Some useful debugging statements:
			//Print_Tree_Screen(tree);
			//if(verbose){
			PhyML_Printf("T: %f iter: %d current lnL = %f proposed lnL = %f\n", tau, iter, lnL_current, lnL_proposed);
			//PhyML_Printf("plot1 %d %lf\n", (steps_tried * (itau + 1)), lnL_current);
			//PhyML_Printf("plot2 %ld %d\n", (long)now, (steps_tried * (itau + 1)));
			PhyML_Printf("plot1 %d %lf\n", steps_tried, lnL_current);
			PhyML_Printf("plot2 %ld %d\n", (long)difftime(now,start), steps_tried);
			//}

			//      PhyML_Printf("JSJ: Proposed Likelihood at iter %i: %lf\n",iter,lnL_current);

			if(lnL_proposed > lnL_best){
				//save this tree into best_tree

				//PhyML_Printf("(annealing.c, 465): Found a new best tree with lnL = %f\n", lnL_best);
				Copy_Tree(tree,best_tree);
				Record_Model(tree->mod, best_tree->mod);

				lnL_best = lnL_proposed;

				tau_of_best = tau;
				iter_of_best = iter;

				// This tau is yielding good results: let's stay here
				// longer, thus increasing our chances of reaching the best ground
				// state at this tau.
				iter -= anneal.set_back;
				if(iter < 0) iter = 0; //make sure not set back into negative...

				acc_prob = 1.0;
				r = 0.0;
			}
			else if(lnL_proposed > lnL_current){
				acc_prob = 1.0;
				r = 0.0;
			}else{
				double distance = Get_distance_between_trees(last_tree, tree);
				acc_prob = Boltzmann_P_QA(lnL_current, lnL_proposed, distance, tau); // VHS: change this to use QA-based Boltzmann
				r = gsl_rng_uniform(anneal.rng);
			}


			if(acc_prob >= r){
				//save the current tree
				if(anneal.no_change == 0){
					Copy_Tree(tree,last_tree);
					Record_Model(tree->mod,last_tree->mod);
				}
				steps_accepted++;
				lnL_current = tree->c_lnL;

				//PhyML_Printf("JSJ: Proposed Likelihood Accepted, acc_prob = %lf\n",acc_prob);

			}else{
				//restore the current tree to be our last tree
				if(anneal.no_change == 0){
					Copy_Tree(last_tree,tree);
					Record_Model(last_tree->mod,tree->mod);
				}
			}
		}//end inner for loop.
		tau *= taumult;

	}
	if(verbose){
		PhyML_Printf("Annealing finished.\n");
		PhyML_Printf("In tau range [%f, %f], the best tree was found at %f, iteration %f\n", tau, anneal.start_tau, tau_of_best, iter_of_best);
	}
	Copy_Tree(best_tree,tree);
	Record_Model(best_tree->mod,tree->mod);

	Free_Model(best_tree->mod);
	Free_Tree(best_tree);
	Free_Model(last_tree->mod);
	Free_Tree(last_tree);
	Free_Anneal(); //local function that frees the gsl_rng;
	Lk(tree);

	return tree->c_lnL;
}

*/
